#ifndef __U2Matrix3_H_A__
#define __U2Matrix3_H_A__

#include "U2PreRequest.h"
#include "U2Math.h"
#include "U2Vector3.h"

// NB All code adapted from Wild Magic 0.2 Matrix math (free source code)
// http://www.geometrictools.com/

// NOTE.  The (x,y,z) coordinate system is assumed to be right-handed.
// Coordinate axis rotation matrices are of the form
//   RX =    1       0       0
//           0     cos(t) -sin(t)
//           0     sin(t)  cos(t)
// where t > 0 indicates a counterclockwise rotation in the yz-plane
//   RY =  cos(t)    0     sin(t)
//           0       1       0
//        -sin(t)    0     cos(t)
// where t > 0 indicates a counterclockwise rotation in the zx-plane
//   RZ =  cos(t) -sin(t)    0
//         sin(t)  cos(t)    0
//           0       0       1
// where t > 0 indicates a counterclockwise rotation in the xy-plane.


U2EG_NAMESPACE_BEGIN

class U2Vector3;
class U2Matrix3;


/** A 3x3 matrix which can represent rotations around axes.
    @note
        <b>All the code is adapted from the Wild Magic 0.2 Matrix
        library (http://www.geometrictools.com/).</b>
    @par
        The coordinate system is assumed to be <b>right-handed</b>.
*/
class _U2Share U2Matrix3
{
public:
    /** Default constructor.
        @note
            It does <b>NOT</b> initialize the matrix for efficiency.
    */
	inline U2Matrix3 () {}
    inline explicit U2Matrix3 (const u2real arr[3][3])
	{
		memcpy(m,arr,9*sizeof(u2real));
	}
    inline U2Matrix3 (const U2Matrix3& rkMatrix)
	{
		memcpy(m,rkMatrix.m,9*sizeof(u2real));
	}
    U2Matrix3 (u2real fEntry00, u2real fEntry01, u2real fEntry02,
                u2real fEntry10, u2real fEntry11, u2real fEntry12,
                u2real fEntry20, u2real fEntry21, u2real fEntry22)
	{
		m[0][0] = fEntry00;
		m[0][1] = fEntry01;
		m[0][2] = fEntry02;
		m[1][0] = fEntry10;
		m[1][1] = fEntry11;
		m[1][2] = fEntry12;
		m[2][0] = fEntry20;
		m[2][1] = fEntry21;
		m[2][2] = fEntry22;
	}

	/** Exchange the contents of this matrix with another. 
	*/
	inline void swap(U2Matrix3& other)
	{
		std::swap(m[0][0], other.m[0][0]);
		std::swap(m[0][1], other.m[0][1]);
		std::swap(m[0][2], other.m[0][2]);
		std::swap(m[1][0], other.m[1][0]);
		std::swap(m[1][1], other.m[1][1]);
		std::swap(m[1][2], other.m[1][2]);
		std::swap(m[2][0], other.m[2][0]);
		std::swap(m[2][1], other.m[2][1]);
		std::swap(m[2][2], other.m[2][2]);
	}

    // member access, allows use of construct mat[r][c]
    inline u2real* operator[] (size_t iRow) const
	{
		return (u2real*)m[iRow];
	}
    /*inline operator u2real* ()
	{
		return (u2real*)m[0];
	}*/
    U2Vector3 GetColumn (size_t iCol) const;
    void SetColumn(size_t iCol, const U2Vector3& vec);
    void FromAxes(const U2Vector3& xAxis, const U2Vector3& yAxis, const U2Vector3& zAxis);

    // assignment and comparison
    inline U2Matrix3& operator= (const U2Matrix3& rkMatrix)
	{
		memcpy(m,rkMatrix.m,9*sizeof(u2real));
		return *this;
	}
    bool operator== (const U2Matrix3& rkMatrix) const;
    inline bool operator!= (const U2Matrix3& rkMatrix) const
	{
		return !operator==(rkMatrix);
	}

    // arithmetic operations
    U2Matrix3 operator+ (const U2Matrix3& rkMatrix) const;
    U2Matrix3 operator- (const U2Matrix3& rkMatrix) const;
    U2Matrix3 operator* (const U2Matrix3& rkMatrix) const;
    U2Matrix3 operator- () const;

    // matrix * vector [3x3 * 3x1 = 3x1]
    U2Vector3 operator* (const U2Vector3& rkVector) const;

    // vector * matrix [1x3 * 3x3 = 1x3]
    inline _U2Share friend U2Vector3 operator* (const U2Vector3& rkVector,
        const U2Matrix3& rkMatrix)
    {
        U2Vector3 kProd;
        for (size_t iRow = 0; iRow < 3; iRow++)
        {
            kProd[iRow] =
                rkVector[0]*rkMatrix.m[0][iRow] +
                rkVector[1]*rkMatrix.m[1][iRow] +
                rkVector[2]*rkMatrix.m[2][iRow];
        }
        return kProd;
    }

    // matrix * scalar
    U2Matrix3 operator* (u2real fScalar) const;

    // scalar * matrix
    inline _U2Share friend U2Matrix3 operator* (u2real fScalar, const U2Matrix3& rkMatrix)
    {
        U2Matrix3 kProd;
        for (size_t iRow = 0; iRow < 3; iRow++)
        {
            for (size_t iCol = 0; iCol < 3; iCol++)
                kProd[iRow][iCol] = fScalar*rkMatrix.m[iRow][iCol];
        }
        return kProd;
    }

    // utilities
    U2Matrix3 Transpose () const;
    bool Inverse (U2Matrix3& rkInverse, u2real fTolerance = 1e-06) const;
    U2Matrix3 Inverse (u2real fTolerance = 1e-06) const;
    u2real Determinant () const;

    // singular value decomposition
    void SingularValueDecomposition (U2Matrix3& rkL, U2Vector3& rkS,
        U2Matrix3& rkR) const;
    void SingularValueComposition (const U2Matrix3& rkL,
        const U2Vector3& rkS, const U2Matrix3& rkR);

    // Gram-Schmidt orthonormalization (applied to columns of rotation matrix)
    void Orthonormalize ();

    // orthogonal Q, diagonal D, upper triangular U stored as (u01,u02,u12)
    void QDUDecomposition (U2Matrix3& rkQ, U2Vector3& rkD,
        U2Vector3& rkU) const;

    u2real SpectralNorm () const;

    // matrix must be orthonormal
    void ToAngleAxis (U2Vector3& rkAxis, Radian& rfAngle) const;
	inline void ToAngleAxis (U2Vector3& rkAxis, Degree& rfAngle) const {
		Radian r;
		ToAngleAxis ( rkAxis, r );
		rfAngle = r;
	}
    void FromAngleAxis (const U2Vector3& rkAxis, const Radian& fRadians);

    // The matrix must be orthonormal.  The decomposition is yaw*pitch*roll
    // where yaw is rotation about the Up vector, pitch is rotation about the
    // Right axis, and roll is rotation about the Direction axis.
    bool ToEulerAnglesXYZ (Radian& rfYAngle, Radian& rfPAngle,
        Radian& rfRAngle) const;
    bool ToEulerAnglesXZY (Radian& rfYAngle, Radian& rfPAngle,
        Radian& rfRAngle) const;
    bool ToEulerAnglesYXZ (Radian& rfYAngle, Radian& rfPAngle,
        Radian& rfRAngle) const;
    bool ToEulerAnglesYZX (Radian& rfYAngle, Radian& rfPAngle,
        Radian& rfRAngle) const;
    bool ToEulerAnglesZXY (Radian& rfYAngle, Radian& rfPAngle,
        Radian& rfRAngle) const;
    bool ToEulerAnglesZYX (Radian& rfYAngle, Radian& rfPAngle,
        Radian& rfRAngle) const;
    void FromEulerAnglesXYZ (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
    void FromEulerAnglesXZY (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
    void FromEulerAnglesYXZ (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
    void FromEulerAnglesYZX (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
    void FromEulerAnglesZXY (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
    void FromEulerAnglesZYX (const Radian& fYAngle, const Radian& fPAngle, const Radian& fRAngle);
    // eigensolver, matrix must be symmetric
    void EigenSolveSymmetric (u2real afEigenvalue[3],
        U2Vector3 akEigenvector[3]) const;

    static void TensorProduct (const U2Vector3& rkU, const U2Vector3& rkV,
        U2Matrix3& rkProduct);

	/** Determines if this matrix involves a scaling. */
	inline bool hasScale() const
	{
		// check magnitude of column vectors (==local axes)
		u2real t = m[0][0] * m[0][0] + m[1][0] * m[1][0] + m[2][0] * m[2][0];
		if (!U2Math::RealEqual(t, 1.0, (u2real)1e-04))
			return true;
		t = m[0][1] * m[0][1] + m[1][1] * m[1][1] + m[2][1] * m[2][1];
		if (!U2Math::RealEqual(t, 1.0, (u2real)1e-04))
			return true;
		t = m[0][2] * m[0][2] + m[1][2] * m[1][2] + m[2][2] * m[2][2];
		if (!U2Math::RealEqual(t, 1.0, (u2real)1e-04))
			return true;

		return false;
	}

	/** Function for writing to a stream.
	*/
	inline _U2Share friend std::ostream& operator <<
		( std::ostream& o, const U2Matrix3& mat )
	{
		o << "U2Matrix3(" << mat[0][0] << ", " << mat[0][1] << ", " << mat[0][2] << ", " 
                        << mat[1][0] << ", " << mat[1][1] << ", " << mat[1][2] << ", " 
                        << mat[2][0] << ", " << mat[2][1] << ", " << mat[2][2] << ")";
		return o;
	}

    static const u2real EPSILON;
    static const U2Matrix3 ZERO;
    static const U2Matrix3 IDENTITY;

protected:
    // support for eigensolver
    void Tridiagonal (u2real afDiag[3], u2real afSubDiag[3]);
    bool QLAlgorithm (u2real afDiag[3], u2real afSubDiag[3]);

    // support for singular value decomposition
    static const u2real msSvdEpsilon;
    static const unsigned int msSvdMaxIterations;
    static void Bidiagonalize (U2Matrix3& kA, U2Matrix3& kL,
        U2Matrix3& kR);
    static void GolubKahanStep (U2Matrix3& kA, U2Matrix3& kL,
        U2Matrix3& kR);

    // support for spectral norm
    static u2real MaxCubicRoot (u2real afCoeff[3]);

    u2real m[3][3];

    // for faster access
    friend class U2Matrix4;
};





U2EG_NAMESPACE_END


#endif
